
#include <cmath>
#include <cstdio>
#include "Movement.h"

namespace Engine {

namespace Kinetic {

Movement::Movement(const Misc::Vector2d & from, const Misc::Vector2d & to, double accel, double friction, double speed, double max)
 : departure(from)
 , position(from)
 , destination(to)
 , velocity((destination - departure).normalize())
 , maxSpeed(max)
 , speed(speed)
 , acceleration(accel)
 , friction(0)
 , targeted(true) {
	if (speed > maxSpeed) speed = maxSpeed;
}


Movement::Movement(const Misc::Vector2d & from, double accel, double friction, double maxSpeed)
 : departure(from)
 , position(from)
 , destination(from)
 , velocity(0, 0)
 , maxSpeed(maxSpeed)
 , speed(speed)
 , acceleration(accel)
 , friction(0)
 , targeted(false) {
	if (speed > maxSpeed) speed = maxSpeed;
}

Movement::Movement() 
 : departure(0.0f, 0.0f)
 , destination(0.0f, 0.0f)
 , velocity(0.0f, 0.0f)
 , maxSpeed(0.0f)
 , speed(0.0f)
 , acceleration(0.0f)
 , friction(0.0f)
 , targeted(false) {
}

Movement::~Movement() {
}

Misc::Vector2d Movement::update(int delta, bool accelerating) {

	
	//std::printf("%.1f %.1f\n", velocity.x, velocity.y);
	//Misc::Vector2d force(0,0);
	/* If we are going towards a target, we should adjust 
	 * acceleration and friction */
	if (targeted) {
		if (((position.distance(departure) >= distanceToBreak) && distanceToCruise <= halfDistance) || (distanceToCruise >= halfDistance && position.distance(departure) >= halfDistance)) {
			if (smooth) {
				/* if we are smoothing things, then we should stop
				 * accelerating and start breaking past-the-half */
				//std::printf("yeaaaaaah");
				accelerating = false;
				friction = acceleration; /* Inverse acceleration */
			}
		} else friction = 0;
	}
	
	//if (targeted) velocity = destination - position;
	
	/* Accelerate */
	if (accelerating) { if (std::abs(speed) < maxSpeed) speed += acceleration*(delta/1000.0f); }
	
	/* Consider friction (+damping) */
	if (speed > 0)
		speed -= friction*(delta/1000.0f);
	else if (speed <= 0)
		speed += friction*(delta/1000.0f);
	
	
	/* Update velocity and position */
	//velocity *= speed;
	position += velocity * speed * (delta/1000.0f);
	if (targeted && (std::abs(destination.x - position.x) < 0.01f) && (std::abs(destination.y - position.y) < 1.0f)) position = destination;
	return position;

}


} // namespace Engine::Kinetic

} // namespace Engine
